clear
%% load the background pictures
img=imread('../rectify/bg_img/background.png');
%% apply projective transform to raw pictures
theta=10;
P=[1 0 0;
   0 1 0;
   0.001 0.001 1];
A=[1 0 0;
    0 1 0;
    0 0 1];
R=[cos(theta) -sin(theta) 200;
   sin(theta) cos(theta) 100;
   0 0 1
  ];
H=A*P;% Only when there is no rotation can it be rectified using five orthgonally crossing lines.
tform = projective2d(H');%The transform matrix in matlab has been previously transposed. 
img_proj=imwarp(img,tform);
imwrite(img_proj,'img_project.png');% save the picture tranformed.
%% Using corner features to register graphs
img_proj=imread('img_project.png');
img_bin=double(img_proj(:,:,1));
lines=div_lines(img_proj(:,:,1),[],[],1);
%% Using five orgonally crossing lines to rectify projective transformation
% Choose five lines, using box index and line index respectively.
% each row represent one crossing lines, using format like
% [box_id,line1_id,line2_id;
%  ...];
line_id=[3 1 4;
         5 6 2;
         13 2 4;
         9 2 4;
         17 4 6;
    ];
% line_id=[3 3 4;
%          15 2 5;
%          9 5 6;
%          7 3 6;
%          2 5 3];
% visualize those having been chosen
[line1,line2]=esti_line(lines,line_id);
line1=line1';
line2=line2';
cross_mat=      [line1(:,1) .* line2(:,1), ...
                (line1(:,1) .* line2(:,2) + line1(:,2) .* line2(:,1))/2 ,...
                line1(:,2) .* line2(:,2), ...
                (line1(:,1) .* line2(:,3) + line1(:,3) .* line2(:,1))/2 ,...
                (line1(:,2) .* line2(:,3) + line1(:,3) .* line2(:,2))/2 ,...
                line1(:,3) .* line2(:,3) ];
[U,S,V]=svd(cross_mat);
b=V(:,end);
% calculate the absolute C*
C_=    [    b(1), b(2)/2, b(4)/2;
            b(2)/2, b(3), b(5)/2;
            b(4)/2, b(5)/2, b(6)];
K_K_T=C_(1:2,1:2);
try
    K_=chol(K_K_T);
catch
    K_=chol(-K_K_T);
end
K_=K_/sqrt(det(K_));

K_K_T_v=[b(4);b(5)];
v=K_\K_K_T_v;

H_=[K_(1,1),K_(1,2),0;
    K_(2,1),K_(2,2),0;
    v(1),v(2),1];
%H_=inv(H);
%[U1,S1,V1]=svd(C_);
tform_rect = projective2d(H_');
img_proj_rect=imwarp(img_proj,tform_rect);
figure(2)
imshow(img_proj_rect);